import math

import open3d as o3d
import yaml
from cuboid_locate import cuboid_locate
from utility import draw_cuboid_corners


def main():
    cfg_file_path = "experiment/tilt_laser_scanner/parallel_config.yaml"
    with open(cfg_file_path, encoding="utf-8") as cfg_file:
        config_data = yaml.load(cfg_file, Loader=yaml.FullLoader)

    pcd_file_path = config_data["pcd_file_path"]
    cuboid_size = config_data["cuboid_size"]
    top_rough_location = config_data["top_rough_location"]
    bottom_rough_location = config_data["bottom_rough_location"]
    spot_distance = config_data["spot_distance"]

    cuboid_length = cuboid_size["length"]
    cuboid_width = cuboid_size["width"]

    # top
    origin_pcd = o3d.io.read_point_cloud(pcd_file_path)
    top_x_range = top_rough_location["x_range"]
    top_y_range = top_rough_location["y_range"]
    top_z_range = top_rough_location["z_range"]

    top_pcd, top_surface = cuboid_locate(origin_pcd, top_x_range, top_y_range,
                                         top_z_range, spot_distance,
                                         cuboid_length, cuboid_width)
    draw_cuboid_corners(top_pcd, top_surface)

    # bottom
    origin_pcd = o3d.io.read_point_cloud(pcd_file_path)
    bottom_x_range = bottom_rough_location["x_range"]
    bottom_y_range = bottom_rough_location["y_range"]
    bottom_z_range = bottom_rough_location["z_range"]

    bottom_pcd, bottom_surface = cuboid_locate(origin_pcd, bottom_x_range,
                                               bottom_y_range, bottom_z_range,
                                               spot_distance, cuboid_length,
                                               cuboid_width)
    draw_cuboid_corners(bottom_pcd, bottom_surface)

    top_right = top_surface.top_right
    bottom_left = bottom_surface.top_left
    distance = (top_right.x - bottom_left.x) ** 2 + \
        (top_right.y - bottom_left.y) ** 2 + \
        (top_right.z - bottom_left.z) ** 2
    distance = math.sqrt(distance)
    print(F"distance is: {distance:.2f}")

    o3d.visualization.draw_geometries([top_pcd, bottom_pcd])


if __name__ == "__main__":
    main()
